#ifndef _MATRIX_TEMPLATE_OPERATORS_H_
#define _MATRIX_TEMPLATE_OPERATORS_H_

#include <stdlib.h>

#include "cuMath.h"

// Side Note : 
// Matrices can be tricky
// they depend a lot on how they are used
// for instance if you want to translate a vector p by xyz value using matrix multiplication
// you have to decide in advance how are you going to multiply your matrix on your vector
// if matrix is the left operand and vector is the right operand
// matrix will have a non zero column4, like this:
// | 1 0 0 x |  |px|
// | 0 1 0 y |  |py|
// | 0 0 1 z |  |pz|
// | 0 0 0 1 |  | 1|
// if matrix is the right operand and vector is the left operand
// matrix will have a non zero row4, like this:
// [px|py|pz| 1] | 1 0 0 0 |  
//               | 0 1 0 0 |  
//               | 0 0 1 0 |  
//               | x y z 1 |  
// more complex functions are even more tricky
// but it always comes to one solution: sometimes you have to transpose your matrix to get it right
// It also doesn't help to eliminate confusion
// that OpenGL stores matrix C O L U M N - wise 
// while multipling it on the R I G H T - side
// which basicaly means that it is a mess.
// So to avoid confusion
// matrix functions that are different depending on operand side
// will have a postfix [R|L] which means that matrix made by this function
// should be used as a [right|left] operand in matrix vector multiplication
// for example:
// TranslateR( &MR ) | TranslateL( &pML )
// will create a matrix that should be used like this:
// v * (MR) | (ML) * v

#include "cuMath.h"

//**********************************************************************************
// matrix4x4 operators and functions
//**********************************************************************************

SDH_CALL
matrix4x4 * Matrix4x4SetByRow ( matrix4x4 * pM4x4, float4 r0, float4 r1, float4 r2, float4 r3 )
{
  if (pM4x4 == NULL) return NULL;

	pM4x4->row[0] = r0;
	pM4x4->row[1] = r1;
	pM4x4->row[2] = r2;
	pM4x4->row[3] = r3;

	return pM4x4;
}

SDH_CALL
matrix4x4 * Matrix4x4SetByColumn ( matrix4x4 * pM4x4, float4 c0, float4 c1, float4 c2, float4 c3 )
{
  if (pM4x4 == NULL) return NULL;

	set( pM4x4->row[0], c0.x, c1.x, c2.x, c3.x );
	set( pM4x4->row[1], c0.y, c1.y, c2.y, c3.y );
	set( pM4x4->row[2], c0.z, c1.z, c2.z, c3.z );
	set( pM4x4->row[3], c0.w, c1.w, c2.w, c3.w );

	return pM4x4;
}

SDH_CALL
matrix4x4 * Matrix4x4SetByValue( matrix4x4 * pM4x4, 
							     float m00, float m01, float m02, float m03, 
							     float m10, float m11, float m12, float m13,
							     float m20, float m21, float m22, float m23,
							     float m30, float m31, float m32, float m33 )
{
  if (pM4x4 == NULL) return NULL;

	set( pM4x4->row[0], m00, m01, m02, m03 );
	set( pM4x4->row[1], m10, m11, m12, m13 );
	set( pM4x4->row[2], m20, m21, m22, m23 );
	set( pM4x4->row[3], m30, m31, m32, m33 );

	return pM4x4;
}

SDH_CALL
matrix4x4 * Matrix4x4SetDiag ( matrix4x4 * pM4x4, float4 vec )
{
  if (pM4x4 == NULL) return NULL;

	set( pM4x4->row[0], vec.x,  0.0f,  0.0f,  0.0f );
	set( pM4x4->row[1], 0.0f,  vec.y,  0.0f,  0.0f );
	set( pM4x4->row[2], 0.0f,   0.0f, vec.z,  0.0f );
	set( pM4x4->row[3], 0.0f,   0.0f,  0.0f, vec.w );

	return pM4x4;
}

SDH_CALL
matrix4x4 * Matrix4x4SetDiag ( matrix4x4 * pM4x4, float3 vec, float w = 1.0f)
{
  if (pM4x4 == NULL) return NULL;

	set( pM4x4->row[0], vec.x,  0.0f,  0.0f,  0.0f );
	set( pM4x4->row[1], 0.0f,  vec.y,  0.0f,  0.0f );
	set( pM4x4->row[2], 0.0f,   0.0f, vec.z,  0.0f );
	set( pM4x4->row[3], 0.0f,   0.0f,  0.0f,     w);

	return pM4x4;
}

SDH_CALL
matrix4x4 * Matrix4x4SetDiag ( matrix4x4 * pM4x4, float v )
{
  if (pM4x4 == NULL) return NULL;

	set( pM4x4->row[0],    v, 0.0f, 0.0f, 0.0f );
	set( pM4x4->row[1], 0.0f,    v, 0.0f, 0.0f );
	set( pM4x4->row[2], 0.0f, 0.0f,    v, 0.0f );
	set( pM4x4->row[3], 0.0f, 0.0f, 0.0f,    v );

	return pM4x4;
}

SDH_CALL
matrix4x4 * Matrix4x4SetDiag ( matrix4x4 * pM4x4, float v1, float v2, float v3, float v4 = 1 )
{
  if (pM4x4 == NULL) return NULL;

	pM4x4->row[0] = f4(   v1, 0.0f, 0.0f, 0.0f );
	pM4x4->row[1] = f4( 0.0f,   v2, 0.0f, 0.0f );
	pM4x4->row[2] = f4( 0.0f, 0.0f,   v3, 0.0f );
	pM4x4->row[3] = f4( 0.0f, 0.0f, 0.0f,   v4 );

	return pM4x4;
}

SDH_CALL
matrix4x4 * Matrix4x4Copy ( matrix4x4 * pDst, matrix4x4 * pSrc) 
{
  if (pDst == NULL || pSrc == NULL) return NULL;

	pDst->row[0] = pSrc->row[0];
	pDst->row[1] = pSrc->row[1];
	pDst->row[2] = pSrc->row[2];
	pDst->row[3] = pSrc->row[3];

	return pDst;
}

SDH_CALL
matrix4x4 * Matrix4x4SetIdentity ( matrix4x4 * pM4x4)
{
  if (pM4x4 == NULL) return NULL;

	pM4x4->row[0] = f4( 1.0f, 0.0f, 0.0f, 0.0f ); 
	pM4x4->row[1] = f4( 0.0f, 1.0f, 0.0f, 0.0f ); 
	pM4x4->row[2] = f4( 0.0f, 0.0f, 1.0f, 0.0f ); 
	pM4x4->row[3] = f4( 0.0f, 0.0f, 0.0f, 1.0f );  

	return pM4x4;
}

SDH_CALL
matrix4x4 Matrix4x4SetIdentity ( matrix4x4 & M4x4)
{
	M4x4.row[0] = f4( 1.0f, 0.0f, 0.0f, 0.0f ); 
	M4x4.row[1] = f4( 0.0f, 1.0f, 0.0f, 0.0f ); 
	M4x4.row[2] = f4( 0.0f, 0.0f, 1.0f, 0.0f ); 
	M4x4.row[3] = f4( 0.0f, 0.0f, 0.0f, 1.0f ); 

	return M4x4;
}

SDH_CALL
matrix4x4 & operator += ( matrix4x4 & M4x4, const matrix4x4& N4x4)
{
	M4x4.row[0] += N4x4.row[0];
	M4x4.row[1] += N4x4.row[1];
	M4x4.row[2] += N4x4.row[2];
	M4x4.row[3] += N4x4.row[3];

	return M4x4;
}

SDH_CALL
matrix4x4 & operator -= ( matrix4x4 & M4x4, const matrix4x4& N4x4)
{
	M4x4.row[0] -= N4x4.row[0];
	M4x4.row[1] -= N4x4.row[1];
	M4x4.row[2] -= N4x4.row[2];
	M4x4.row[3] -= N4x4.row[3];

	return M4x4;
}

SDH_CALL
matrix4x4 & operator *= ( matrix4x4 & M4x4, const matrix4x4& N4x4)
{
	float4 column[4] = {
		{N4x4.row[0].x, N4x4.row[1].x, N4x4.row[2].x, N4x4.row[3].x},
		{N4x4.row[0].y, N4x4.row[1].y, N4x4.row[2].y, N4x4.row[3].y},
		{N4x4.row[0].z, N4x4.row[1].z, N4x4.row[2].z, N4x4.row[3].z},
		{N4x4.row[0].w, N4x4.row[1].w, N4x4.row[2].w, N4x4.row[3].w},
	};

	matrix4x4 temp = M4x4;

	M4x4.row[0].x = dot(temp.row[0], column[0]);
	M4x4.row[0].y = dot(temp.row[0], column[1]);
	M4x4.row[0].z = dot(temp.row[0], column[2]);
	M4x4.row[0].w = dot(temp.row[0], column[3]);

	M4x4.row[1].x = dot(temp.row[1], column[0]);
	M4x4.row[1].y = dot(temp.row[1], column[1]);
	M4x4.row[1].z = dot(temp.row[1], column[2]);
	M4x4.row[1].w = dot(temp.row[1], column[3]);

	M4x4.row[2].x = dot(temp.row[2], column[0]);
	M4x4.row[2].y = dot(temp.row[2], column[1]);
	M4x4.row[2].z = dot(temp.row[2], column[2]);
	M4x4.row[2].w = dot(temp.row[2], column[3]);

	M4x4.row[3].x = dot(temp.row[3], column[0]);
	M4x4.row[3].y = dot(temp.row[3], column[1]);
	M4x4.row[3].z = dot(temp.row[3], column[2]);
	M4x4.row[3].w = dot(temp.row[3], column[3]);

	return M4x4;
}

SDH_CALL
matrix4x4& operator *= ( matrix4x4 & M4x4, float f)
{
	M4x4.row[0] *= f;
	M4x4.row[1] *= f;
	M4x4.row[2] *= f;
	M4x4.row[3] *= f;

	return M4x4;
}

SDH_CALL
matrix4x4& operator /= ( matrix4x4 & M4x4, float f)
{
	M4x4.row[0] /= f;
	M4x4.row[1] /= f;
	M4x4.row[2] /= f;
	M4x4.row[3] /= f;

	return M4x4;
}

SDH_CALL
matrix4x4 operator + (const matrix4x4 & L4x4, const matrix4x4 & R4x4)
{
	matrix4x4 M4x4;

	M4x4.row[0] = L4x4.row[0] + R4x4.row[0];
	M4x4.row[1] = L4x4.row[1] + R4x4.row[1];
	M4x4.row[2] = L4x4.row[2] + R4x4.row[2];
	M4x4.row[3] = L4x4.row[3] + R4x4.row[3];

	return M4x4;
}

SDH_CALL
matrix4x4 operator - ( const matrix4x4 & L4x4, const matrix4x4 & R4x4)
{
	matrix4x4 M4x4;

	M4x4.row[0] = L4x4.row[0] - R4x4.row[0];
	M4x4.row[1] = L4x4.row[1] - R4x4.row[1];
	M4x4.row[2] = L4x4.row[2] - R4x4.row[2];
	M4x4.row[3] = L4x4.row[3] - R4x4.row[3];

	return M4x4;
}

SDH_CALL
matrix4x4 operator * ( const matrix4x4 & L4x4, const matrix4x4 & R4x4)
{
	matrix4x4 M4x4;

	float4 column[4] = {
		{R4x4.row[0].x, R4x4.row[1].x, R4x4.row[2].x, R4x4.row[3].x},
		{R4x4.row[0].y, R4x4.row[1].y, R4x4.row[2].y, R4x4.row[3].y},
		{R4x4.row[0].z, R4x4.row[1].z, R4x4.row[2].z, R4x4.row[3].z},
		{R4x4.row[0].w, R4x4.row[1].w, R4x4.row[2].w, R4x4.row[3].w},
	};

	M4x4.row[0].x = dot(L4x4.row[0], column[0]);
	M4x4.row[0].y = dot(L4x4.row[0], column[1]);
	M4x4.row[0].z = dot(L4x4.row[0], column[2]);
	M4x4.row[0].w = dot(L4x4.row[0], column[3]);

	M4x4.row[1].x = dot(L4x4.row[1], column[0]);
	M4x4.row[1].y = dot(L4x4.row[1], column[1]);
	M4x4.row[1].z = dot(L4x4.row[1], column[2]);
	M4x4.row[1].w = dot(L4x4.row[1], column[3]);

	M4x4.row[2].x = dot(L4x4.row[2], column[0]);
	M4x4.row[2].y = dot(L4x4.row[2], column[1]);
	M4x4.row[2].z = dot(L4x4.row[2], column[2]);
	M4x4.row[2].w = dot(L4x4.row[2], column[3]);

	M4x4.row[3].x = dot(L4x4.row[3], column[0]);
	M4x4.row[3].y = dot(L4x4.row[3], column[1]);
	M4x4.row[3].z = dot(L4x4.row[3], column[2]);
	M4x4.row[3].w = dot(L4x4.row[3], column[3]);

	return M4x4;
}

SDH_CALL
matrix4x4 operator * ( const matrix4x4& N4x4, float f)
{
	matrix4x4 M4x4;
	M4x4.row[0] = N4x4.row[0] * f;
	M4x4.row[1] = N4x4.row[1] * f;
	M4x4.row[2] = N4x4.row[2] * f;
	M4x4.row[3] = N4x4.row[3] * f;

	return M4x4;
}

SDH_CALL
matrix4x4 operator * ( float f, const matrix4x4& N4x4)
{
	matrix4x4 M4x4;
	M4x4.row[0] = N4x4.row[0] * f;
	M4x4.row[1] = N4x4.row[1] * f;
	M4x4.row[2] = N4x4.row[2] * f;
	M4x4.row[3] = N4x4.row[3] * f;

	return M4x4;
}

SDH_CALL
float4  operator * ( const matrix4x4& M4x4, const float4& v)
{
	float4 u;

	u.x = dot(M4x4.row[0], v);
	u.y = dot(M4x4.row[1], v);
	u.z = dot(M4x4.row[2], v);
	u.w = dot(M4x4.row[3], v);

	return u;
}

SDH_CALL
float4  operator * ( const float4& v, const matrix4x4& M4x4)
{
	float4 u;

	float4 column[4] = {
		{M4x4.row[0].x, M4x4.row[1].x, M4x4.row[2].x, M4x4.row[3].x},
		{M4x4.row[0].y, M4x4.row[1].y, M4x4.row[2].y, M4x4.row[3].y},
		{M4x4.row[0].z, M4x4.row[1].z, M4x4.row[2].z, M4x4.row[3].z},
		{M4x4.row[0].w, M4x4.row[1].w, M4x4.row[2].w, M4x4.row[3].w},
	};

	u.x = dot(column[0], v);
	u.y = dot(column[1], v);
	u.z = dot(column[2], v);
	u.w = dot(column[3], v);

	return u;
}

SDH_CALL
float4  operator * ( const matrix4x4& M4x4, const float3& v)
{
	float4 u;

	u.x = dot(M4x4.row[0], v) + M4x4.row[0].w;
	u.y = dot(M4x4.row[1], v) + M4x4.row[1].w;
	u.z = dot(M4x4.row[2], v) + M4x4.row[2].w;
	u.w = dot(M4x4.row[3], v) + M4x4.row[3].w;

	return u;
}

SDH_CALL
float4  operator * ( const float3& v, const matrix4x4& M4x4)
{
	float4 u;

	float4 column[4] = {
		{M4x4.row[0].x, M4x4.row[1].x, M4x4.row[2].x, M4x4.row[3].x},
		{M4x4.row[0].y, M4x4.row[1].y, M4x4.row[2].y, M4x4.row[3].y},
		{M4x4.row[0].z, M4x4.row[1].z, M4x4.row[2].z, M4x4.row[3].z},
		{M4x4.row[0].w, M4x4.row[1].w, M4x4.row[2].w, M4x4.row[3].w},
	};

	u.x = dot(column[0], v);
	u.y = dot(column[1], v);
	u.z = dot(column[2], v);
	u.w = dot(column[3], v);

	return u;
}

SDH_CALL matrix4x4 * Matrix4x4Transpose(matrix4x4 * pDst, matrix4x4 * pSrc)
{
    return Matrix4x4SetByValue( pDst,
                                pSrc->row[0].x, pSrc->row[1].x, pSrc->row[2].x, pSrc->row[3].x, 
                                pSrc->row[0].y, pSrc->row[1].y, pSrc->row[2].y, pSrc->row[3].y, 
                                pSrc->row[0].z, pSrc->row[1].z, pSrc->row[2].z, pSrc->row[3].z, 
                                pSrc->row[0].w, pSrc->row[1].w, pSrc->row[2].w, pSrc->row[3].w);
}

SDH_CALL float Matrix4x4Determinant( matrix4x4 * pM4x4 ) 
{
  if (pM4x4 == NULL) return NULL;

	float m00 = pM4x4->row[0].x, m01 = pM4x4->row[0].y, m02 = pM4x4->row[0].z, m03 = pM4x4->row[0].w;
	float m10 = pM4x4->row[1].x, m11 = pM4x4->row[1].y, m12 = pM4x4->row[1].z, m13 = pM4x4->row[1].w;
	float m20 = pM4x4->row[2].x, m21 = pM4x4->row[2].y, m22 = pM4x4->row[2].z, m23 = pM4x4->row[2].w;
	float m30 = pM4x4->row[3].x, m31 = pM4x4->row[3].y, m32 = pM4x4->row[3].z, m33 = pM4x4->row[3].w;

	float determinant = 	
	m03*m12*m21*m30 - m02*m13*m21*m30 - m03*m11*m22*m30 + m01*m13*m22*m30+
	m02*m11*m23*m30 - m01*m12*m23*m30 - m03*m12*m20*m31 + m02*m13*m20*m31+
	m03*m10*m22*m31 - m00*m13*m22*m31 - m02*m10*m23*m31 + m00*m12*m23*m31+
	m03*m11*m20*m32 - m01*m13*m20*m32 - m03*m10*m21*m32 + m00*m13*m21*m32+
	m01*m10*m23*m32 - m00*m11*m23*m32 - m02*m11*m20*m33 + m01*m12*m20*m33+
	m02*m10*m21*m33 - m00*m12*m21*m33 - m01*m10*m22*m33 + m00*m11*m22*m33;
	
	return determinant;
}

SDH_CALL matrix4x4 * Matrix4x4Inverse( matrix4x4 * pDst, matrix4x4 * pSrc ) 
{
  if (pSrc == NULL || pDst == NULL) return NULL;

	float m00 = pSrc->row[0].x, m01 = pSrc->row[0].y, m02 = pSrc->row[0].z, m03 = pSrc->row[0].w;
	float m10 = pSrc->row[1].x, m11 = pSrc->row[1].y, m12 = pSrc->row[1].z, m13 = pSrc->row[1].w;
	float m20 = pSrc->row[2].x, m21 = pSrc->row[2].y, m22 = pSrc->row[2].z, m23 = pSrc->row[2].w;
	float m30 = pSrc->row[3].x, m31 = pSrc->row[3].y, m32 = pSrc->row[3].z, m33 = pSrc->row[3].w;

	float determinant = 	
	m03*m12*m21*m30 - m02*m13*m21*m30 - m03*m11*m22*m30 + m01*m13*m22*m30+
	m02*m11*m23*m30 - m01*m12*m23*m30 - m03*m12*m20*m31 + m02*m13*m20*m31+
	m03*m10*m22*m31 - m00*m13*m22*m31 - m02*m10*m23*m31 + m00*m12*m23*m31+
	m03*m11*m20*m32 - m01*m13*m20*m32 - m03*m10*m21*m32 + m00*m13*m21*m32+
	m01*m10*m23*m32 - m00*m11*m23*m32 - m02*m11*m20*m33 + m01*m12*m20*m33+
	m02*m10*m21*m33 - m00*m12*m21*m33 - m01*m10*m22*m33 + m00*m11*m22*m33;

	if (abs(determinant) < 0.0f)
	{		
		return NULL;
	}
	pDst->row[0].x = m11*m22*m33 + m12*m23*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 - m13*m22*m31;
    pDst->row[0].y = m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33;
    pDst->row[0].z = m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33;
    pDst->row[0].w = m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23;
    pDst->row[1].x = m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33;
    pDst->row[1].y = m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33;
    pDst->row[1].z = m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33;
    pDst->row[1].w = m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23;
    pDst->row[2].x = m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33;
    pDst->row[2].y = m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33;
    pDst->row[2].z = m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33;
    pDst->row[2].w = m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23;
    pDst->row[3].x = m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32;
    pDst->row[3].y = m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32;
    pDst->row[3].z = m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32;
    pDst->row[3].w = m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22;
   
	(*pDst) /= (determinant);

	return pDst;
}

SDH_CALL matrix4x4 * Matrix4x4Scale ( matrix4x4 * pM4x4, float sx, float sy, float sz, float sw = 1.0f)
{
  if (pM4x4 == NULL) return NULL;

	Matrix4x4SetDiag( pM4x4, sx, sy, sz, sw);

	return pM4x4;
}

SDH_CALL matrix4x4 * Matrix4x4Scale ( matrix4x4 * pM4x4, const float3 & v3, float sw = 1.0f)
{
  if (pM4x4 == NULL) return NULL;

	Matrix4x4SetDiag( pM4x4, v3, sw );

	return pM4x4;
}

SDH_CALL matrix4x4 * Matrix4x4Scale ( matrix4x4 * pM4x4, const float4 & v4)
{
  if (pM4x4 == NULL) return NULL;

	Matrix4x4SetDiag( pM4x4, v4);

	return pM4x4;
}

// RotateAxis() as in OpenGL
// More on how this function works: http://pyopengl.sourceforge.net/documentation/manual/glRotate.3G.html
SDH_CALL matrix4x4 * Matrix4x4RotateAxisL ( matrix4x4 * pM4x4, const float3 & u, float angle )
{
  if (pM4x4 == NULL) return NULL;

  float3 v = normalize(u);

	float c = (float)cos ( angle );
	float s = (float)sin ( angle );

	pM4x4->row[0].x = v.x * v.x * (1 - c) + c;
	pM4x4->row[0].y = v.x * v.y * (1 - c) - s * v.z;
	pM4x4->row[0].z = v.x * v.z * (1 - c) + s * v.y;
	pM4x4->row[0].w = 0.0f;

	pM4x4->row[1].x = v.y * v.x * (1 - c) + s * v.z;
	pM4x4->row[1].y = v.y * v.y * (1 - c) + c;
	pM4x4->row[1].z = v.y * v.z * (1 - c) - s * v.x;
	pM4x4->row[1].w = 0.0f;

	pM4x4->row[2].x = v.z * v.x * (1 - c) - s * v.y;
	pM4x4->row[2].y = v.z * v.y * (1 - c) + s * v.x;
	pM4x4->row[2].z = v.z * v.z * (1 - c) + c;
	pM4x4->row[2].w = 0.0f;

	pM4x4->row[3].x = pM4x4->row[3].y = pM4x4->row[3].z = 0.0f;
	pM4x4->row[3].w = 1;

	return pM4x4;
}

SDH_CALL matrix4x4 * Matrix4x4RotateAxisR ( matrix4x4 * pM4x4, const float3 & u, float angle )
{
  if (pM4x4 == NULL) return NULL;

  float3 v = normalize(u);

	float c = (float)cos ( angle );
	float s = (float)sin ( angle );

	pM4x4->row[0].x = v.x * v.x * (1 - c) + c;
	pM4x4->row[1].x = v.x * v.y * (1 - c) - s * v.z;
	pM4x4->row[2].x = v.x * v.z * (1 - c) + s * v.y;
	pM4x4->row[3].x = 0.0f;

	pM4x4->row[0].y = v.y * v.x * (1 - c) + s * v.z;
	pM4x4->row[1].y = v.y * v.y * (1 - c) + c;
	pM4x4->row[2].y = v.y * v.z * (1 - c) - s * v.x;
	pM4x4->row[3].y = 0.0f;

	pM4x4->row[0].z = v.z * v.x * (1 - c) - s * v.y;
	pM4x4->row[1].z = v.z * v.y * (1 - c) + s * v.x;
	pM4x4->row[2].z = v.z * v.z * (1 - c) + c;
	pM4x4->row[3].z = 0.0f;

	pM4x4->row[0].w = pM4x4->row[1].w = pM4x4->row[2].w = 0.0f;
	pM4x4->row[3].w = 1;

	return pM4x4;
}

SDH_CALL matrix4x4 * Matrix4x4RotateYawPithRollR ( matrix4x4 * pM4x4, float yaw, float pitch, float roll )
{	
  if (pM4x4 == NULL) return NULL;

	matrix4x4 mYaw, mPitch, mRoll;
	
	Matrix4x4RotateAxisR ( &mPitch, f3(1, 0, 0), pitch );
	Matrix4x4RotateAxisR ( &mYaw,   f3(0, 1, 0), yaw );
	Matrix4x4RotateAxisR ( &mRoll,  f3(0, 0, 1), roll );

	pM4x4[0] =  mRoll * mPitch * mYaw;

	return pM4x4;
}

SDH_CALL matrix4x4 * Matrix4x4RotateYawPithRollL ( matrix4x4 * pM4x4, float yaw, float pitch, float roll )
{	
  if (pM4x4 == NULL) return NULL;

	matrix4x4 mYaw, mPitch, mRoll;
	
	Matrix4x4RotateAxisL ( &mPitch, f3(1, 0, 0), pitch );
	Matrix4x4RotateAxisL ( &mYaw,   f3(0, 1, 0), yaw   );
	Matrix4x4RotateAxisL ( &mRoll,  f3(0, 0, 1), roll  );

	pM4x4[0] = mYaw * mPitch * mRoll;

	return pM4x4;
}

SDH_CALL
matrix4x4 * Matrix4x4PerspectiveRH( matrix4x4 * pM4x4, float w, float h, float zn, float zf )
{
  float invz = zf / ( zf - zn );

  return Matrix4x4SetByValue( pM4x4,
							  2.0f * zn / w, 0.0f,           0.0f,       0.0f,
							  0.0f,          2.0f * zn / h,  0.0f,       0.0f,
							  0.0f,          0.0f,          -invz,      -1.0f,
							  0.0f,          0.0f,          -zn * invz,  0.0f );
}

SDH_CALL 
matrix4x4 * Matrix4x4PerspectiveLH( matrix4x4 * pM4x4, float w, float h, float zn, float zf )
{
  float invz = zf / ( zf - zn );

  return Matrix4x4SetByValue( pM4x4,
							  2.0f * zn / w, 0.0f,           0.0f,      0.0f,
							  0.0f,          2.0f * zn / h,  0.0f,      0.0f,
							  0.0f,          0.0f,           invz,      1.0f,
							  0.0f,          0.0f,          -zn * invz, 0.0f );
}

SDH_CALL 
matrix4x4 * Matrix4x4PerspectiveFovRH( matrix4x4 * pM4x4, float fovy, float aspect, float zn, float zf )
{
  float cotangent = 1.0f / tanf( fovy * 0.5f );
  float invz = zf / ( zn - zf );

  return Matrix4x4SetByValue( pM4x4,
							  cotangent / aspect, 0.0f,      0.0f,       0.0f,
							  0.0f,               cotangent, 0.0f,       0.0f,
							  0.0f,               0.0f,      invz,      -1.0f,
							  0.0f,               0.0f,      zn * invz,  0.0f );
}

SDH_CALL 
matrix4x4 * Matrix4x4PerspectiveFovLH( matrix4x4 * pM4x4, float fovy, float aspect, float zn, float zf )
{
  float cotangent = 1.0f / tanf( fovy * 0.5f );
  float invz = zf / ( zf - zn );

  return Matrix4x4SetByValue( pM4x4,
	                          cotangent / aspect, 0.0f,       0.0f,      0.0f,
    						  0.0f,               cotangent,  0.0f,      0.0f,
							  0.0f,               0.0f,       invz,      1.0f,
							  0.0f,               0.0f,      -zn * invz, 0.0f );
}

SDH_CALL 
matrix4x4 * Matrix4x4PerspectiveOffCenterRH( matrix4x4 * pM4x4, float l, float r, float b, float t, float zn, float zf )
{
  float invz = zf / (zf - zn);
  float invw = 1.0f / (r - l);
  float invh = 1.0f / (t - b);

  return Matrix4x4SetByValue( pM4x4, 
                              2.0f * zn * invw, 0.0f,              0.0f,       0.0f,
                              0.0f,             2.0f * zn * invh,  0.0f,       0.0f,
                              (l + r) * invw,   (t + b) * invh,   -invz,      -1.0f,
                              0.0f,             0.0f,             -zn * invz,  0.0f );
}

SDH_CALL 
matrix4x4 * Matrix4x4PerspectiveOffCenterLH( matrix4x4 * pM4x4, float l, float r, float b, float t, float zn, float zf )
{
  float invz = zf / ( zf - zn );
  float invw = 1.0f / ( r - l );
  float invh = 1.0f / ( t - b );

  return Matrix4x4SetByValue( pM4x4, 
                              2.0f * zn * invw,   0.0f,              0.0f,      0.0f,
                              0.0f,               2.0f * zn * invh,  0.0f,      0.0f,
                              -(l + r) * invw,    -(t + b) * invh,   invz,      1.0f,
                              0.0f,               0.0f,             -zn * invz, 0.0f );
}

SDH_CALL 
matrix4x4 * Matrix4x4OrthoRH( matrix4x4 * pM4x4, float w, float h, float zn, float zf )
{
  float invz = 1.0f / ( zf - zn );

  return Matrix4x4SetByValue( pM4x4, 
                              2.0f / w,  0.0f,      0.0f,      0.0f,
                              0.0f,      2.0f / h,  0.0f,      0.0f,
                              0.0f,      0.0f,     -invz,      0.0f,
                              0.0f,      0.0f,     -zn * invz, 1.0f );
}

SDH_CALL 
matrix4x4 * Matrix4x4OrthoLH( matrix4x4 * pM4x4, float w, float h, float zn, float zf )
{
  float invz = 1.0f / (zf - zn);

  return Matrix4x4SetByValue( pM4x4, 
                              2.0f / w,  0.0f,      0.0f,      0.0f,
                              0.0f,      2.0f / h,  0.0f,      0.0f,
                              0.0f,      0.0f,      invz,      0.0f,
                              0.0f,      0.0f,     -zn * invz, 1.0f );
}

SDH_CALL 
matrix4x4 * Matrix4x4OrthoOffCenterRH( matrix4x4 * pM4x4, float l, float r, float b, float t, float zn, float zf )
{
  float invz = 1.0f / ( zf - zn );
  float invw = 1.0f / ( r - l );
  float invh = 1.0f / ( t - b );

  return Matrix4x4SetByValue( pM4x4, 
                              2.0f * invw,      0.0f,             0.0f,      0.0f,
                              0.0f,             2.0f * invh,      0.0f,      0.0f,
                              0.0f,             0.0f,            -invz,      0.0f,
                              -(l + r) * invw, -(t + b) * invh,  -zn * invz, 1.0f );
}

SDH_CALL 
matrix4x4 * Matrix4x4OrthoOffCenterLH( matrix4x4 * pM4x4, float l, float r, float b, float t, float zn, float zf )
{   
  float invz = 1.0f / ( zf - zn );
  float invw = 1.0f / ( r - l );
  float invh = 1.0f / ( t - b );

  return Matrix4x4SetByValue( pM4x4,
                              2.0f * invw,      0.0f,            0.0f,      0.0f,
                              0.0f,             2.0f * invh,     0.0f,      0.0f,
                              0.0f,             0.0f,            invz,      0.0f,
                              -(l + r) * invw, -(t + b) * invh,  -zn * invz, 1.0f );
}

SDH_CALL
matrix4x4 * Matrix4x4TranslateR( matrix4x4 * pM4x4, float3 v )
{
  if (pM4x4 == NULL) return NULL;

	set( pM4x4->row[0], 1.0f, 0.0f, 0.0f, 0.0f ); 
	set( pM4x4->row[1], 0.0f, 1.0f, 0.0f, 0.0f ); 
	set( pM4x4->row[2], 0.0f, 0.0f, 1.0f, 0.0f ); 
	set( pM4x4->row[3],  v.x,  v.y,  v.z, 1.0f ); 

	return pM4x4;
}

SDH_CALL
matrix4x4 * Matrix4x4TranslateL( matrix4x4 * pM4x4, float3 v )
{
  if (pM4x4 == NULL) return NULL;

	set( pM4x4->row[0], 1.0f, 0.0f, 0.0f,  v.x ); 
	set( pM4x4->row[1], 0.0f, 1.0f, 0.0f,  v.y ); 
	set( pM4x4->row[2], 0.0f, 0.0f, 1.0f,  v.z ); 
	set( pM4x4->row[3], 0.0f, 0.0f, 0.0f, 1.0f ); 

	return pM4x4;
}
// Transform Point Right Multiplication Rule for float3 float4
SDH_CALL 
float3 Matrix4x4TransformPointL( const matrix4x4 & M4x4, const float3 & p )
{
	return f3( M4x4.row[0].x*p.x + M4x4.row[0].y*p.y + M4x4.row[0].z*p.z + M4x4.row[0].w,   // p.w assumed to be 1.0f
		         M4x4.row[1].x*p.x + M4x4.row[1].y*p.y + M4x4.row[1].z*p.z + M4x4.row[1].w,
	           M4x4.row[2].x*p.x + M4x4.row[2].y*p.y + M4x4.row[2].z*p.z + M4x4.row[2].w );
}

SDH_CALL 
float4 Matrix4x4TransformPointL( const matrix4x4 & M4x4, const float4 & p )
{
    return f4( M4x4.row[0].x*p.x + M4x4.row[0].y*p.y + M4x4.row[0].z*p.z + M4x4.row[0].w*p.w,
               M4x4.row[1].x*p.x + M4x4.row[1].y*p.y + M4x4.row[1].z*p.z + M4x4.row[1].w*p.w,
               M4x4.row[2].x*p.x + M4x4.row[2].y*p.y + M4x4.row[2].z*p.z + M4x4.row[2].w*p.w, 
               M4x4.row[3].x*p.x + M4x4.row[3].y*p.y + M4x4.row[3].z*p.z + M4x4.row[3].w*p.w);
}


// Transform Direction Left Multiplication Rule for float3 float4 
SDH_CALL 
float3 Matrix4x4TransformDirectionL( const matrix4x4 & M4x4, const float3 & d )
{
	return f3( M4x4.row[0].x*d.x + M4x4.row[0].y*d.y + M4x4.row[0].z*d.z,
		         M4x4.row[1].x*d.x + M4x4.row[1].y*d.y + M4x4.row[1].z*d.z,
	           M4x4.row[2].x*d.x + M4x4.row[2].y*d.y + M4x4.row[2].z*d.z );
}

SDH_CALL 
float4 Matrix4x4TransformDirectionL( const matrix4x4 & M4x4, const float4 & d )
{
    return f4( M4x4.row[0].x*d.x + M4x4.row[0].y*d.y + M4x4.row[0].z*d.z,
               M4x4.row[1].x*d.x + M4x4.row[1].y*d.y + M4x4.row[1].z*d.z,
               M4x4.row[2].x*d.x + M4x4.row[2].y*d.y + M4x4.row[2].z*d.z, 0 );
}

// Transform Point Right Multiplication Rule for float3 float4 
SDH_CALL 
float3 Matrix4x4TransformPointR( const float3 & p, const matrix4x4 & M4x4)
{
	return f3( M4x4.row[0].x*p.x + M4x4.row[1].x*p.y + M4x4.row[2].x*p.z + M4x4.row[3].x,
		         M4x4.row[0].y*p.x + M4x4.row[1].y*p.y + M4x4.row[2].y*p.z + M4x4.row[3].y,
	           M4x4.row[0].z*p.x + M4x4.row[1].z*p.y + M4x4.row[2].z*p.z + M4x4.row[3].z );
}

SDH_CALL 
float4 Matrix4x4TransformPointR( const float4 & p, const matrix4x4 & M4x4)
{
    return f4( M4x4.row[0].x*p.x + M4x4.row[1].x*p.y + M4x4.row[2].x*p.z + M4x4.row[3].x * p.w,
               M4x4.row[0].y*p.x + M4x4.row[1].y*p.y + M4x4.row[2].y*p.z + M4x4.row[3].y * p.w,
               M4x4.row[0].z*p.x + M4x4.row[1].z*p.y + M4x4.row[2].z*p.z + M4x4.row[3].z * p.w, 
               M4x4.row[0].w*p.x + M4x4.row[1].w*p.y + M4x4.row[2].w*p.z + M4x4.row[3].w * p.w);
}

// Transform Direction Right Multiplication Rule for float3 float4 
SDH_CALL 
float3 Matrix4x4TransformDirectionR( const float3 & d, const matrix4x4 & M4x4 )
{
	return f3( M4x4.row[0].x*d.x + M4x4.row[1].x*d.y + M4x4.row[2].x*d.z,
		         M4x4.row[0].y*d.x + M4x4.row[1].y*d.y + M4x4.row[2].y*d.z,
	           M4x4.row[0].z*d.x + M4x4.row[1].z*d.y + M4x4.row[2].z*d.z );
}

SDH_CALL 
float4 Matrix4x4TransformDirectionR( const float4 & d, const matrix4x4 & M4x4 )
{
    return f4( M4x4.row[0].x*d.x + M4x4.row[1].x*d.y + M4x4.row[2].x*d.z,
               M4x4.row[0].y*d.x + M4x4.row[1].y*d.y + M4x4.row[2].y*d.z,
               M4x4.row[0].z*d.x + M4x4.row[1].z*d.y + M4x4.row[2].z*d.z, 0 );
}

// Matrix4x4LookAt() as in OpenGL
// More on how this function works: http://pyopengl.sourceforge.net/documentation/manual/gluLookAt.3G.html
SDH_CALL matrix4x4 * Matrix4x4LookAtLHL( matrix4x4 * pM4x4, const float3 & Eye, const float3 & At, const float3 & Up )
{
    float3 F = normalize(At - Eye);
    float3 S = normalize(cross(F, Up));
    float3 U = cross(S, F);
    
		matrix4x4 transform;
    Matrix4x4SetByValue( &transform,
                           S.x,  S.y,  S.z, 0.0f,
                           U.x,  U.y,  U.z, 0.0f,
                          -F.x, -F.y, -F.z, 0.0f,
                          0.0f, 0.0f, 0.0f, 1.0f);
		matrix4x4 translate;
		Matrix4x4TranslateL( &translate, -Eye);

		*pM4x4 = transform * translate;

		return pM4x4;
}

SDH_CALL matrix4x4 * Matrix4x4LookAtLHR( matrix4x4 * pM4x4, const float3 & Eye, const float3 & At, const float3 & Up )
{
	float3 F = normalize(At - Eye);
	float3 S = normalize(cross(F, Up));
	float3 U = cross(S, F);

	matrix4x4 transform;
	Matrix4x4SetByValue( &transform,
												S.x,  S.y,  S.z, 0.0f,
												U.x,  U.y,  U.z, 0.0f,
												-F.x, -F.y, -F.z, 0.0f,
												0.0f, 0.0f, 0.0f, 1.0f);

	Matrix4x4SetByValue( &transform,
												S.x,  U.x,  -F.x, 0.0f,
												S.y,  U.y,  -F.y, 0.0f,
												S.z,  U.z,  -F.z, 0.0f,
												0.0f, 0.0f, 0.0f, 1.0f);
	matrix4x4 translate;
	Matrix4x4TranslateR( &translate, -Eye);

	*pM4x4 = translate * transform;

	return pM4x4;
}

SDH_CALL matrix4x4 * Matrix4x4LookAtRHL( matrix4x4 * pM4x4, const float3 & Eye, const float3 & At, const float3 & Up )
{
    float3 zaxis = normalize(Eye - At);
    float3 xaxis = normalize(cross(Up, zaxis));
    float3 yaxis = cross(zaxis, xaxis);
    
    return Matrix4x4SetByValue( pM4x4,
                                xaxis.x, xaxis.y, xaxis.z, -dot(xaxis, Eye),
                                yaxis.x, yaxis.y, yaxis.z, -dot(yaxis, Eye),
                                zaxis.x, zaxis.y, zaxis.z, -dot(zaxis, Eye),
                                   0.0f,    0.0f,    0.0f,            1.0f);
}

SDH_CALL matrix4x4 * Matrix4x4LookAtRHR( matrix4x4 * pM4x4, const float3 & Eye, const float3 & At, const float3 & Up )
{
    float3 zaxis = normalize(Eye - At);
    float3 xaxis = normalize(cross(Up, zaxis));
    float3 yaxis = cross(zaxis, xaxis);
    
    return Matrix4x4SetByValue( pM4x4,
                                 xaxis.x,          yaxis.x,          zaxis.x,         0.0f,
                                 xaxis.y,          yaxis.y,          zaxis.y,         0.0f,
                                 xaxis.z,          yaxis.z,          zaxis.z,         0.0f,
                                -dot(xaxis, Eye), -dot(yaxis, Eye), -dot(zaxis, Eye), 1.0f);
}

// Matrix4x4Perspective() as in OpenGL
// More on how this function works: http://pyopengl.sourceforge.net/documentation/manual/gluPerspective.3G.html
SDH_CALL matrix4x4 * Matrix4x4PerspectiveFovGLR( matrix4x4 * pM4x4, float fovy, float aspect, float zn, float zf )
{
  float f = 1.0f / tanf( to_radian(fovy) * 0.5f );
  float invz = 1.0f / ( zn - zf );

  return Matrix4x4SetByValue( pM4x4,
							  f/aspect, 0.0f,   0.0f,            0.0f,
							  0.0f,        f,   0.0f,            0.0f,
                              0.0f,     0.0f,  (zn+zf)*invz,    -1.0f,
							  0.0f,     0.0f,   2.0f*zn*zf*invz, 0.0f );
}

SDH_CALL matrix4x4 * Matrix4x4PerspectiveFovGLL( matrix4x4 * pM4x4, float fovy, float aspect, float zn, float zf )
{
  float f = 1.0f / tanf( fovy * 0.5f );
  float invz = 1.0f / ( zn - zf );

  return Matrix4x4SetByValue( pM4x4,
							  f/aspect, 0.0f,   0.0f,        0.0f,
							  0.0f,        f,   0.0f,        0.0f,
                              0.0f,     0.0f,  (zn+zf)*invz, 2.0f*zn*zf*invz,
							  0.0f,     0.0f,  -1.0f,        0.0f );
}

#endif // _MATRIX_TEMPLATE_OPERATORS_H_
